from pylab import *
from visual import *
import random
import math

SCALE = 0.1
GRAPH_VALUES = 100

# GRAPH
ion()       	# Turn on interactive mode.
#hold(True) 	# Clear the plot before adding new data.
grid(True)
title('IMU Data')
ylabel('angle (rad)')
xlabel('time (s)')
xs = []
ys = []
zs = []
graph1,  = plot(xs, ys)
graph2, = plot(xs, zs, 'c')

# CUBE
display(autocenter=False, x=0, y=0, center=(0,0,1), background=(0,0,0))
box = box()
box.velocity = vector(0,0,3)

moving = False
x = 0
while True:
	# GRAPH
	if moving:
		xs.pop(0)
		ys.pop(0)
		zs.pop(0)
		axis((xs[0], xs[GRAPH_VALUES-1], -1, 1))
	else:
		if len(xs) > GRAPH_VALUES:
			moving = True
		xlim(0, GRAPH_VALUES*SCALE)
		ylim(-1, 1)
	xs.append(x)
	y = math.cos(x)
	ys.append(y)
	zs.append(math.sin(x)+random.uniform(0,1))
	graph1.set_xdata(xs)
	graph1.set_ydata(ys)
	graph2.set_xdata(xs)
	graph2.set_ydata(zs)
	x += SCALE
	
	# CUBE
	rate(10)
	r = random.uniform(0,360)
	box.rotate(angle=pi/r, axis=(x,r,r), origin=(0,0,0))
